#include "sgf.h"

int main()
{
  unsigned char rv = 0;


  unsigned char ctext[123];

  unsigned char key[] = "aaaaaaaaaaaaaaaaaa";
  unsigned char ptext[] = "8777777777777777777778";
  unsigned char dtext[123];

  memset(ctext, 0, 123);
  memset(dtext, 0, 123);

  shiftLeft(ptext, strlen(ptext)*8);
  printf(":%s:\n", ptext);
  shiftRight(ptext, strlen(ptext)*8);
  printf(":%s:\n", ptext);

  sgfe(key, ptext, ctext);
  sgfd(key, ctext, CTEXTLENGTH((int)strlen(ptext)), dtext);
  printf(":%s:\n", dtext);
	return rv;
}


//************************************
// Method:    sgfe
// FullName:  encrypt plain text
// Access:    public 
// Returns:   void
// Qualifier:
// Parameter: unsigned char* key    a key input by user
// Parameter: unsigned char* ptext  the plain text
// Parameter: unsigned char* ctext  encrypted text, its size must be greater than ptext's and integer multiple of BLOCK_SIZE_B
//************************************
void sgfe(unsigned char* key, unsigned char* ptext, unsigned char* ctext)
{
  int i = 0;
  int j = 0;
  int nbytes = (int)strlen(ptext);
  unsigned char block[BLOCK_SIZE_B];
  unsigned char subkeys[ROUNDS][BLOCK_SIZE_B / 2];


  // generate subkeys
  subKeyGenerator(ROUNDS, key, subkeys);

  // encrypt ptext block by block
  for (i = 0; i < nbytes - BLOCK_SIZE_B; i += BLOCK_SIZE_B)
  {
    sgfe_block(subkeys, ptext + i, ctext + i);
  }
  memset(block, 0, BLOCK_SIZE_B);
  memcpy(block, ptext + i, nbytes - i);
  sgfe_block(subkeys, block, ctext + i);
} // sgfe

//************************************
// Method:    sgfe_block
// FullName:  encrypte a block of the plain text
// Access:    public 
// Returns:   void
// Qualifier:
// Parameter: unsigned char* key    a key input by user
// Parameter: unsigned char* ptext  a block of the plain text
// Parameter: unsigned char* ctext  encrypted text
//************************************
void sgfe_block(unsigned char subkeys[ROUNDS][BLOCK_SIZE_B / 2], unsigned char* ptext, unsigned char* ctext)
{
  int i = 0;
  unsigned char l[BLOCK_SIZE_B / 2];
  unsigned char r[BLOCK_SIZE_B / 2];
  unsigned char t[BLOCK_SIZE_B / 2];
  
  // init l & r
  memcpy(l, ptext, BLOCK_SIZE_B / 2);
  memcpy(r, ptext + BLOCK_SIZE_B / 2, BLOCK_SIZE_B / 2);

  // run ROUNDS times
  for (i = 0; i < ROUNDS; i++)
  {
    memcpy(t, r, BLOCK_SIZE_B / 2);
    f(subkeys[i], r);
    xorbits(r, l, BLOCK_SIZE_b / 2);
    memcpy(l, t, BLOCK_SIZE_B / 2);
  }

  // generate ctext
  memcpy(ctext, r, BLOCK_SIZE_B / 2);
  memcpy(ctext + BLOCK_SIZE_B / 2, l, BLOCK_SIZE_B / 2);
} // sgfe_block


//************************************
// Method:    f
// FullName:  the round function
// Access:    public 
// Returns:   void
// Qualifier:
// Parameter: unsigned char* key    a subkey
// Parameter: unsigned char* text   the input text
//************************************
void f(unsigned char* key, unsigned char* text)
{

  // 1. shift left
  shiftLeft(text, BLOCK_SIZE_b / 2);

  // 2. text XOR key
  xorbits(text, key, BLOCK_SIZE_b / 2);

} // f

//************************************
// Method:    sgfd
// FullName:  decrypt ctext
// Access:    public 
// Returns:   void
// Qualifier:
// Parameter: unsigned char* key    the key input by user
// Parameter: unsigned char* ctext  the encrypted text
// Parameter: int nbytes            the number of bytes in ctext, integer multiple of BLOCK_SIZE_B
// Parameter: unsigned char* dtext  the decrypted text
//************************************
void sgfd(unsigned char* key, unsigned char* ctext, int nbytes, unsigned char* dtext)
{
  int i = 0;
//  unsigned char block[BLOCK_SIZE_B];
  unsigned char subkeys[ROUNDS][BLOCK_SIZE_B / 2];

  // generate subkeys
  subKeyGenerator(ROUNDS, key, subkeys);

  // decrypt ctext block by block
  for (i = 0; i < nbytes; i += BLOCK_SIZE_B)
  {
    sgfd_block(subkeys, ctext + i, dtext + i);
  }
//  memset(block, 0, BLOCK_SIZE_B);
//  memcpy(block, ctext + i, nbytes - i);
//  sgfd_block(subkeys, block, dtext + i);
}

//************************************
// Method:    sgfd_block
// FullName:  decrypt a block of encrypted text
// Access:    public 
// Returns:   void
// Qualifier:
// Parameter: unsigned char subkeys   subkeys array
// Parameter: unsigned char* cblock   encrypted block
// Parameter: unsigned char* dblock   decrypted block
//************************************
void sgfd_block(unsigned char subkeys[ROUNDS][BLOCK_SIZE_B / 2], unsigned char* cblock, unsigned char* dblock)
{
  int i = 0;
  unsigned char l[BLOCK_SIZE_B / 2];
  unsigned char r[BLOCK_SIZE_B / 2];
  unsigned char t[BLOCK_SIZE_B / 2];

  // init l & r
  memcpy(r, cblock, BLOCK_SIZE_B / 2);
  memcpy(l, cblock + BLOCK_SIZE_B / 2, BLOCK_SIZE_B / 2);

  // run ROUNDS times
  for (i = ROUNDS - 1; i >= 0; i--)
  {
    memcpy(t, l, BLOCK_SIZE_B / 2);
    f(subkeys[i], l);
    xorbits(l, r, BLOCK_SIZE_b / 2);
    memcpy(r, t, BLOCK_SIZE_B / 2);
  }

  // generate ctext
  memcpy(dblock, l, BLOCK_SIZE_B / 2);
  memcpy(dblock + BLOCK_SIZE_B / 2, r, BLOCK_SIZE_B / 2);
}

//************************************
// Method:    subKeyGenerator
// FullName:  subKeyGenerator
// Access:    public 
// Returns:   void
// Qualifier:
// Parameter: int rounds            encryption rounds
// Parameter: unsigned char* key    the key input by user, a string with \0 as its end.
// Parameter: unsigned char subkeys generated subkeys
//************************************
void subKeyGenerator(int rounds, unsigned char* key, unsigned char subkeys[][BLOCK_SIZE_B / 2])
{
  int i = 0;
  int j = 0;
  int n = (int)strlen(key); // key length
  // left half of a subkey
  unsigned char l[BLOCK_SIZE_B / 4];
  // right half of a subkey
  unsigned char r[BLOCK_SIZE_B / 4];
  // expanded key
  unsigned char expkey[BLOCK_SIZE_B / 2];

  // expand key to size of BLOCK_SIZE_B
  if (n >= BLOCK_SIZE_B / 2)
  {
    memcpy(expkey, key, BLOCK_SIZE_B / 2);
  } 
  else
  {
    // fill up expkey
    for (i = 0; i < BLOCK_SIZE_B / 2 - n;)
    {
      memcpy(expkey + i, key, n);
      i += n;
    }
    if (i < BLOCK_SIZE_B / 2)
    {
      memcpy(expkey + i, key, BLOCK_SIZE_B / 2 - i);
    }
  }

  //puts(key);
  //for (i = 0; i < BLOCK_SIZE_B / 2; i++)
  //{
  //  printf("%x ", expkey[i]);
  //}
  //printf("\n");


  // change each byte
  for(i = 1; i < BLOCK_SIZE_B / 2; i++)
  {
    expkey[i] ^= expkey[i-1] + expkey[i];
  }

  memcpy(l, expkey, BLOCK_SIZE_B / 4);
  memcpy(r, expkey + BLOCK_SIZE_B / 4, BLOCK_SIZE_B / 4);

  for (i = 0; i < ROUNDS; i++)
  {
    memcpy(subkeys[i], l, BLOCK_SIZE_B/4);
    memcpy(&subkeys[i][BLOCK_SIZE_B/4], r, BLOCK_SIZE_B/4);

    for (j = 0; j < BLOCK_SIZE_B / 2 - 1; j++)
    {
      subkeys[i][j] += subkeys[i][j+1];
    }
    subkeys[i][j] += subkeys[i][0];

    memcpy(l, subkeys[i], BLOCK_SIZE_B/4);
    memcpy(r, &subkeys[i][BLOCK_SIZE_B/4], BLOCK_SIZE_B/4);    
  }

  /*for (i = 0; i < ROUNDS; i++)
  {
    for (j = 0; j < BLOCK_SIZE_B / 2; j++)
    {
      printf("%x ", subkeys[i][j]);
    }
    printf("\n");
  }*/
}

//************************************
// Method:    xorbits
// FullName:  xorbits
// Access:    public 
// Returns:   void
// Qualifier:
// Parameter: unsigned char* b1   data to be XORed. the result is placed here.
// Parameter: unsigned char* b2   data for XOR
// Parameter: int nbits           the number of bits (not byte)
//************************************
void xorbits(unsigned char* b1, unsigned char* b2, int nbits)
{
	int i = 0;
  int j = 0;
  unsigned char c1;
  unsigned char c2;

	for (i = 0; i * 8 <= nbits - 8; i++)
	{
		b1[i] ^= b2[i];
	}

  if (i * 8 < nbits)
  {
    j = nbits - i * 8;
    c1 = b1[i] & ((1<<j) - 1);
    c2 = b2[i] & ((1<<j) - 1);
    c1 ^= c2;
    b1[i] &= ~((1<<j) - 1);
    b1[i] |= c1;
  }

} // xorbits


//************************************
// shift bits left, high bits <-- low bits
// Method:    shiftLeft
// FullName:  shiftLeft
// Access:    public 
// Returns:   void
// Qualifier:
// Parameter: unsigned char* bits   data to be shifted left
// Parameter: int nbits             the number of bits (not bytes)
//************************************
void shiftLeft(unsigned char* bits, int nbits)
{
  int i = 0;
  int hr = 0;
  int hl = 0;
  unsigned char c;

  // shift left bytes
  for (i = 0; i * 8 <= nbits - 8; i++)
  {
    hl = bits[i] >> 7;
    bits[i] = bits[i] << 1;
    // move the highest bit of the right byte to this byte
    bits[i] |= hr;
    hr = hl;
  }
  
  // handle the extra bits, whose amount is less than 8
  if (i * 8 < nbits)
  {
    c = bits[i];
    
    // shift left
    c = c << 1;
    // zero the invalid bits
    c &= (1<<(nbits-i*8)) - 1;
    
    // zero the valid bits
    bits[i] &= ~((1<<(nbits-i*8))-1);

    bits[i] |= c;
  }

  // move the highest bit of the highest byte to the lowest byte.
  bits[0] |= hr;
} // shiftLeft



//************************************
// shift bits right, high bits --> low bits
// Method:    shiftRight
// FullName:  shiftRight
// Access:    public 
// Returns:   void
// Qualifier:
// Parameter: unsigned char* bits   data to be shifted right
// Parameter: int nbits             the number of bits (not bytes)
//************************************
void shiftRight(unsigned char* bits, int nbits)
{
  int i = 0;
  int hr = 0;
  int hl = 0;
  unsigned char invalid = 0;

  // handle the highest byte first
  // get the first bit of the highest byte
  hr = bits[(nbits - 1) / 8] & 0x1;
  // get invalid bits
  invalid = nbits%8==0?0:(~((1 << (nbits%8)) - 1)) & bits[(nbits - 1) / 8];
  // clear invalid bits
  bits[(nbits - 1) / 8] &= nbits%8==0?0xFF:(1 << (nbits%8)) - 1;
  // shift right the highest byte
  bits[(nbits - 1) / 8] >>= 1;
  // copy old invalid bits' data back
  bits[(nbits - 1) / 8] |= invalid;
  // the highest bit will be handled at the end.
  
  // shift right bytes
  hl = hr;
  for (i = (nbits-1) / 8 - 1; i >= 0; i--)
  {
    hr = bits[i] & 0x1;
    bits[i] >>= 1;
    // move the highest bit of the left byte to this byte
    bits[i] |= hl << 7;
    hl = hr;
  }

  // handle the highest bit
  if (hl == 0)
  {
    CLEARBIT(bits, nbits - 1);
  } 
  else
  {
    SETBIT(bits, nbits - 1);
  }

} // shiftRight

